void init_terminal() {
    copy_16bit_code(go_text, go_text_end, GO_TEXT_CS);
    set_fgbg(WHITE, BLACK);
    clear();
}

void go_text_mode() {
    general_registers blank;
    go_v86(GO_TEXT_CS, 0x0000, blank);
    restore_buffer();
}

void put_cursor_at(int x, int y) {
    u16int cursorLocation = y * 80 + x;
    outb(0x3D4, 14);
    outb(0x3D5, cursorLocation >> 8);
    outb(0x3D4, 15);
    outb(0x3D5, cursorLocation);
}

void put_character_at(char c, int x, int y, u8int foreground, u8int background) {
    u16int final = vidmem_char_for(c, foreground, background);
    video_memory[y*80 + x] = final;
    video_buffer[y*80 + x] = final;
}

void scroll_buffer(char c, u8int foreground, u8int background) {
    u16int final = vidmem_char_for(c, foreground, background);
    memcpy16(video_memory, video_memory + 80, 80*24);
    memset16(video_memory + 80*24, final, 80);
    memcpy16(video_buffer, video_buffer + 80, 80*24);
    memset16(video_buffer + 80*24, final, 80);
}

void fill_buffer(char c, u8int foreground, u8int background) {
    u16int final = vidmem_char_for(c, foreground, background);
    memset16(video_memory, final, 80*25);
    memset16(video_buffer, final, 80*25);
}

u16int vidmem_char_for(char c, u8int foreground, u8int background) {
    u8int color = (background << 4) | (foreground & 0x0F);
    u8int character = c;
    u16int final = (color << 8) | character;
}

void set_cursor(int x, int y) {
    cursor_x = x;
    cursor_y = y;
    put_cursor_at(x, y);
}

void get_cursor(int* x, int* y) {
    *x = cursor_x;
    *y = cursor_y;
}

void put_char(char c) {
    if(c == '\r' || c == '\n') {
        cursor_x = 0;
        cursor_y++;
        if(cursor_y == 25) {
            cursor_y = 24;
            scroll_buffer(' ', 0xF, 0x0);
        }
    }
    if(c == 0x7F || c == '\b') {
        dec_cursor();
        put_character_at(' ', cursor_x, cursor_y, foreground, background);
    }
    else if(c >= 0x20) {
        put_character_at(c, cursor_x, cursor_y, foreground, background);
        inc_cursor();
    }
    put_cursor_at(cursor_x, cursor_y);
}

void put_string(char* c) {
    int i = 0;
    while(c[i] != '\0') {
        put_char(c[i]);
        i++;
    }
}

void put_dec(u32int d) {
    put_string(itoa(d, 10));
}

void put_padded_dec(u32int d, int padding) {
    char* string = itoa(d, 10);
    int i = 0;
    for(i = padding-strlen(string); i > 0; i--)
        put_char('0');
    put_string(string);
}

void put_hex(u32int d) {
    put_string(itoa(d, 0x10));
}

void put_padded_hex(u32int d, int padding) {
    char* string = itoa(d, 0x10);
    int i = 0;
    for(i = padding-strlen(string); i > 0; i--)
        put_char('0');
    put_string(string);
}

void put_bool(bool b) {
    if(b == TRUE)
        put_string(" true");
    else
        put_string("false");
}

output_colors get_colors() {
    output_colors colors;
    colors.foreground = foreground;
    colors.background = background;
    return colors;
}

void set_colors(output_colors colors) {
    foreground = colors.foreground;
    background = colors.background;
}

void set_fgbg(u8int fg, u8int bg) {
    foreground = fg;
    background = bg;
}

void clear() {
    cursor_x = 0;
    cursor_y = 0;
    fill_buffer(' ', foreground, background);
    put_cursor_at(cursor_x, cursor_y);
}

void endl() {
    put_char('\n');
}

void dec_cursor() {
    if(cursor_x > 0) {
        cursor_x--;
    }
}

void inc_cursor() {
    cursor_x++;
    if(cursor_x == 80) {
        cursor_x = 0;
        cursor_y++;
        if(cursor_y == 25) {
            cursor_y = 24;
            scroll_buffer(' ', 0xF, 0x0);
        }
    }
}

void preserve_cursor() {
    get_cursor(&preserved_x, &preserved_y);
}

void restore_cursor() {
    set_cursor(preserved_x, preserved_y);
}

void preserve_colors() {
    preserved_colors = get_colors();
}

void restore_colors() {
    set_colors(preserved_colors);
}

void preserve_formatting() {
    preserve_colors();
    preserve_cursor();
}

void restore_formatting() {
    restore_colors();
    restore_cursor();
}

void preserve_buffer() {
    memcpy16(video_buffer, video_memory, 80*25);
}

void restore_buffer() {
    memcpy16(video_memory, video_buffer, 80*25);
}

void printf(char* control_string, ...) {
    arguments_start = &control_string + 1;
    int i = 0;
    int n = 0;
    char c = ' ';
    for(i = 0; i < strlen(control_string); i++) {
        c = control_string[i];
        if(c == '%') {
            c = control_string[++i];
            switch(c) {
                case 's':
                    put_string(stringArgumentNumber(n++));
                    break;
                case 'd':
                    put_dec(intArgumentNumber(n++));
                    break;
                case 'x':
                    put_hex(intArgumentNumber(n++));
                    break;
                case 'c':
                    put_char(charArgumentNumber(n++));
                    break;
                case 'p':
                    put_string("0x");
                    put_padded_hex(intArgumentNumber(n++), 8);
                    break;
                default:
                    put_char(c);
            }
        }
        else {
            put_char(c);
        }
    }
}

void* argumentLocation(int i) {
    return arguments_start + (4 * i);
}

char* stringArgumentNumber(int i) {
    char** argument_location = (char**)argumentLocation(i);
    return *argument_location;
}

int intArgumentNumber(int i) {
    int* argument_location = (int*)argumentLocation(i);
    return *argument_location;
}

char charArgumentNumber(int i) {
    char* argument_location = (char*)argumentLocation(i);
    return *argument_location;
}

void debug_isr_stack(isr_stack stack) {
    if(stack.eflags & EFLAGS_VM)
        debug_v86_isr_stack(stack);    
    else
        debug_normal_isr_stack(stack);
}

void debug_normal_isr_stack(isr_stack stack) {
    set_fgbg(WHITE, RED);
    clear();
    printf("--------------------------\n");
    printf("RHINOVIRUS SCREEN OF DEATH\n");
    printf("--------------------------\n");
    endl();
    printf("Fatal Exception: %d (%d)\n", stack.interrupt, stack.error_code);
    endl();
    printf("CS:EIP: 0x%x:%x\n", stack.cs, stack.eip);
    printf("DS:     0x%x\n", stack.ds);
    printf("EFLAGS: 0x%x\n", stack.eflags);
    endl();
    printf("DSI:    0x%x\n", stack.dsi);
    printf("ESI:    0x%x\n", stack.esi);
    printf("EBP:    0x%x\n", stack.ebp);
    printf("ESP:    0x%x\n", stack.esp);
    endl();
    printf("EAX:    0x%x\n", stack.eax);
    printf("EBX:    0x%x\n", stack.ebx);
    printf("ECX:    0x%x\n", stack.ecx);
    printf("EDX:    0x%x\n", stack.edx);
    endl();
    printf("STACK DUMP (ESP and Earlier):\n");
    stack_dump(3, (u32int*)stack.esp);
    draw_character(rhinovirus);
}

void debug_v86_isr_stack(isr_stack stack) {
    set_fgbg(WHITE, BLUE);
    clear();
    printf("--------------------------\n");
    printf("RHINOVIRUS SCREEN OF DEATH\n");
    printf("--------------------------\n");
    endl();
    printf("Fatal Exception: %d (%d) While in Virtual 8086 Mode\n", stack.interrupt, stack.error_code);
    endl();
    printf("CS:IP:   0x%x:%x\n", stack.cs, stack.eip);
    printf("EFLAGS:  0x%x\n", stack.eflags);
    printf("EAX:     0x%x\n", stack.eax);
    printf("EBX:     0x%x\n", stack.ebx);
    printf("ECX:     0x%x\n", stack.ecx);
    printf("EDX:     0x%x\n", stack.edx);
    endl();
    printf("V86 ESP: 0x%x\n", stack.v86_sp);
    printf("V86 SS:  0x%x\n", stack.v86_ss);
    printf("V86 ES:  0x%x\n", stack.v86_es);
    printf("V86 DS:  0x%x\n", stack.v86_ds);
    printf("V86 FS:  0x%x\n", stack.v86_fs);
    printf("V86 GS:  0x%x\n", stack.v86_gs);
    endl();
    printf("STACK DUMP (V86 ESP and Earlier; Linear Address 0x%x):\n", (stack.v86_ss * 0x10 + stack.v86_sp));
    stack_dump(3, (u32int*)(stack.v86_ss * 0x10 + stack.v86_sp));
    draw_character(rhinovirus);
}

void stack_dump(int rows, u32int* location) {
    for(int j = 0; j < rows; j++) {
        for(int i = 0; i < 7; i++) {
            printf("%p ", *(location + 7*j + i));
        }
        endl();
    }
}

void draw_character(char** character) {
    preserve_cursor();
    for(int i = 0; i < 13; i++) {
        set_cursor(25, 6+i);
        put_string(character[i]);
    }
    restore_cursor();
}
